#include <ch9.h>
#include <Max3421e.h>
#include <Max3421e_constants.h>
#include <Max_LCD.h>
#include <Usb.h>

#include <AndroidAccessory.h>

AndroidAccessory acc("Manufacturer",
"Model",
"Description",
"1.0",
"edu.uidaho.RG",
"0000000012345678");

int sensorPin = A0;    // select the input pin for the potentiometer
int sensorPin2 = A2;
int ledPin = 13;      // select the pin for the LED - onboard arduino LED
int sensorValue = 0;
int sensorValue2 = 0;

//Feedback LEDs
int LED1 = 30;
int LED2 = 44;

void setup() {

  // declare the ledPin as an OUTPUT:

  Serial.begin(115200);
  //Serial.println("Start");

  pinMode(ledPin, OUTPUT);

  //Setup the Feedback LEDs for output and ground the cathode
  pinMode(LED1, OUTPUT);
  pinMode(31, OUTPUT); 
  digitalWrite(31,LOW);
  pinMode(LED2, OUTPUT);
  pinMode(45, OUTPUT); 
  digitalWrite(45,LOW);

  acc.powerOn();
}

void loop()
{
  int MSGLEN = 11;
  
  byte msgOut[3]; // Output Fixed length message of format 0XX (Zero-Variable-Variable)
  byte msgIn[11]; // Input Fixed length message of format 0XX[4][4] (Zero-Variable-Variable-int-int)
  int inputint[2]; //array of the two inputs
  float received_setpoint[2]; // setpoint scaled between 0 and 100.00 (floating point)
  

  //Get potentiometer input and scale it
  sensorValue = analogRead(sensorPin);
  sensorValue2 = analogRead(sensorPin2);
  sensorValue = map(sensorValue, 0, 1023, 1, 125); // change?
  sensorValue2 = map(sensorValue2, 0, 1023, 1, 125);

  if (acc.isConnected())
  {
    //Serial.println("Acc successfully conneced");
    /*******
     *SEND
     ******/
    msgOut[0] = 0;
    msgOut[1] = sensorValue;
    msgOut[2] = sensorValue2;
    acc.write(msgOut, 3);
    //    delay(DelayValue);

    /*******
     *RECEIVE
     ******/
    int len = acc.read(msgIn, sizeof(msgIn), MSGLEN); // read data into msgIn variable [ 0, x, x, 4byte, 4byte ]
    if (len > MSGLEN - 1) { 
      Serial.println("Length > 0");
      if (msgIn[0] == 0){ // Check correct protocol starting value of 0 (Zero)
        //Light two feedback LEDs corresponding to input received
        if (msgIn[1] == 1) digitalWrite(LED1,HIGH); // turn on light
        else digitalWrite(LED1,LOW); // turn off light

        if (msgIn[2] == 1)digitalWrite(LED2,HIGH); // turn on light
        else digitalWrite(LED2,LOW); // turn off light

        //convert first 4byte array into its integer value (0 - 10,000) and scale back
        inputint[0] = toInt(&msgIn[3]); // send the int starting at offset 3
        inputint[1] = toInt(&msgIn[7]); // send the int starting at offset 3

        for(int i = 0; i < 2; i++){
          received_setpoint[i] = inputint[i] / 100.0;
          Serial.print("SetPoint "); 
          Serial.print(i); 
          Serial.print(": "); 
          Serial.print(received_setpoint[i]);
          Serial.println(" :: ");

        }
        Serial.println("");
      }
    }
    else{
      //Serial.println("No msg received"); 
    }

    //Serial.println(sensorValue, HEX);

    //delay(50); 

  }
  else{
    //Serial.println("Acc unsuccessful on connection"); 
  }
}

int toInt(byte in[]){
  int a[4];

  for(int i = 0; i < 4; i++){
    a[i] = int(in[i]);
  }
  return a[0] << 24 | a[1] << 16 | a[2] << 8 | a[3]; // bit shift and or together
}












